/*
safty.c
offer agv car check abstacle on driving derection and deal with function
designed by lunanting

---------------------------
2020-05-25 10:11:26
create file
---------------------------
*/
#include "safty.h"
#include "io.h"
#include "aos/kernel.h"
#include "upper.h"
#include "../bsp/cli_cmd.h"
#ifdef AC
#include "../driver/pansonic/wheel.h"
#include "../driver/lidar/linux_lidar.h"
static WHEEL_object *wheel;
#endif

static SAFTY_class safty={0};

void xp_safty_check_thread(void *arg);

/*
safty mode init
*/
void xp_safty_init(void){
    aos_task_t id;

    if(safty.init==1)return;
    xp_agv_io_init(1);
    aos_task_new_ext(&id,"safty check", xp_safty_check_thread, NULL, 1500, 30);
    safty.init=1;
    println("safty module inited!");
}


/*
check safty state
*/
#if 1
SAFTY_STATE_E xp_safty_check(uint8_t lidar_roi, int32_t obstacle_dist){
	int32_t safe_dist = obstacle_dist + 10;//安全距离=避障距离+10cm
	int32_t lidar_dist = 0;
	static uint32_t lidar_err_cnt = 0;
	static uint32_t lidar_ok_cnt = 0;
	static uint32_t obstacle_cnt = 0;
	static uint32_t no_obstacle_cnt = 0;
	static uint32_t too_near_cnt = 0;
	static bool lidar_err_flag = false;
	static long long last_blow_time = 0;//上次吹气时刻
	static uint8_t blow_count = 0;//连续吹气次数
	static SAFTY_STATE_E safe_state = SAFTY_state_normal;

	lidar_dist = xp_linux_lidar_dis_get();

	//先进行雷达是否正常工作的判断
	if(255 == lidar_dist){
		lidar_ok_cnt = 0;
		lidar_err_cnt++;
		if(lidar_err_cnt > 0){                      //雷达距离异常时，直接停止不滤波，防止异常值255参与距离调节
			lidar_err_flag = true;
			println("v_lidar is abnormal!");
			xp_upper_v_lidar_state_set(255);
			safe_state = SAFTY_state_lidar_abnormal;
			return safe_state;
		}else{
			return safe_state;//返回之前的状态
		}
	}else{
		lidar_err_cnt = 0;
		lidar_ok_cnt++;
		if(true == lidar_err_flag){
			if(lidar_ok_cnt > 3){
				lidar_err_flag = false;
				xp_upper_v_lidar_state_set(0);
				println("====v_lidar is recover!");
			}
			else{
				println("====wait v_lidar recover!");
				safe_state = SAFTY_state_lidar_abnormal;
				return safe_state;
			}
		}else{
			xp_upper_v_lidar_state_set(0);
		}
	}

	//雷达正常情况下进行安全避障的判断
	if(LIDAR_roi_round == lidar_roi){
		lidar_dist = xp_linux_lidar_dis_get();
	}else if(LIDAR_roi_leave == lidar_roi){
		lidar_dist = xp_linux_lidar_dis2_get();
	}else if(LIDAR_roi_obstacle == lidar_roi){
		lidar_dist = xp_linux_lidar_dis3_get();
	}
	if(lidar_dist < obstacle_dist){
		no_obstacle_cnt = 0;
		obstacle_cnt++;
		if(obstacle_cnt > 3){
			xp_upper_stoped_set(1);
			if((obstacle_cnt > 10) && (xp_time_get_diff_ms(last_blow_time) > 3500) && (blow_count < 5)){
				//如果一直遇障，则需要吹气，尝试把雷达表面水滴吹干，最多连续吹5次，每次吹气间隔时间超过3.5s
				xp_linux_lidar_blow();
				last_blow_time = aos_now_ms();
				blow_count++;
			}
			
			println("safty_check: lidar_dist %d < %d, obstacle!", lidar_dist, safe_dist);
			safe_state = SAFTY_state_obstacle;
			return safe_state;
		}else{
			return safe_state;//返回之前的状态
		}
	}else{
		obstacle_cnt = 0;
		no_obstacle_cnt++;
		if(no_obstacle_cnt > 3){
			blow_count = 0;
			xp_upper_stoped_set(0);
			if(lidar_dist < safe_dist){
				//大于避障距离小于安全距离为过近
				too_near_cnt++;
				if(too_near_cnt > 5){
					println("safty_check: %d < lidar_dist %d < %d, too near!", obstacle_dist, lidar_dist, safe_dist);
					safe_state = SAFTY_state_too_near;
				}
			}else{
				too_near_cnt = 0;
				safe_state = SAFTY_state_normal;
			}
			return safe_state;
		}else{
			return safe_state;//返回之前的状态
		}
	}
}
#else
void xp_safty_check(void){
    static int dir_fw=0,dir_bk=0;
    static u64 count_fw[2]={0},count_bk[2]={0};
    u8 sta[2]={0};
    static u8 mag_flag = 0,unmag_flag=0;
    static s8 dir=0,flag=0;
    static int speeds[2]={0},speed=0;
    if(safty.init!=1)return;
    //前进方向检测
    sta[0]=xp_agv_io_read(SAFTY_FW_1);
    sta[1]=xp_agv_io_read(SAFTY_FW_2);
    if(sta[0]==0||sta[1]==0){
        count_fw[1]=aos_now_ms();
        if((count_fw[1]-count_fw[0])>SAFTY_touch_time){

            /*
            if(xp_wheel_get_dir()>0){
                dir_fw=xp_wheel_get_dir();
                xp_wheel_move(1,0);
            }
            */
            if(safty.fw!=1){
                safty.fw=1;
#ifdef BATTERY_VERSION
                aos_mutex_lock(&g_mutex_mag_navigation,AOS_WAIT_FOREVER);
                if(1 == xp_mag_get_mag_flag())
                {
                xp_mag_move_start_or_stop(0);
                mag_flag =1;
                }
                else if(xp_wheel_get_dir()==1){
                unmag_flag=1;
                xp_wheel_set_wheel_speed(0,0);
                }
                aos_mutex_unlock(&g_mutex_mag_navigation); 
#elif defined AC
				wheel=xp_wheel_object_get();
                if(wheel->dir!=0&&flag==0){
                    dir=wheel->dir;
					(wheel->speed[0]>wheel->speed[0])?(speed=wheel->speed[0]):(speed=wheel->speed[1]);
                    xp_wheel_move(0,0);
                    flag=1;
                }
                if((wheel->speeds[0]!=0||wheel->speeds[1]!=0)&&flag==0){
                    speeds[0]=wheel->speeds[0];
                    speeds[1]=wheel->speeds[1];
                    xp_wheel_motorSpeed_set(0,0);
                    flag=2;
                }
				xp_wheel_set_wheel_locker(1);
#endif
                xp_upper_stoped_set(1);
                println("safty mode fw touched !,sta[0]=%d,sta[1]=%d",sta[0],sta[1]);
            }
        }
    }
    else{
        count_fw[0]=aos_now_ms();
        if((count_fw[0]-count_fw[1])>SAFTY_relase_touch_time){
            /*
            if(dir_fw>0){
                dir_fw=0;
                xp_wheel_move(1,1);
            }
            */
            if(safty.fw!=0){
                safty.fw=0;
#ifdef BATTERY_VERSION
                aos_mutex_lock(&g_mutex_mag_navigation,AOS_WAIT_FOREVER);
                 if(1 == xp_mag_get_mag_flag()|| mag_flag == 1)
                 {
                    xp_mag_move_start_or_stop(1);
                    mag_flag =0;
                 }
                 else if(xp_wheel_get_dir()==1||unmag_flag==1){
                    unmag_flag=0;
                    xp_wheel_set_wheel_speed(0.1,0.1);
                 }
                 aos_mutex_unlock(&g_mutex_mag_navigation);
#elif defined AC
                 if(flag==1){
                    xp_wheel_move(speed,dir);
                 }
                 if(flag==2){
                    xp_wheel_motorSpeed_set(speeds[0],speeds[1]);
                 }
				 xp_wheel_set_wheel_locker(0);
                 flag=0;
#endif
                 xp_upper_stoped_set(0);
                println("safty mode fw touch released !");
            }
#if defined AC
            xp_wheel_set_wheel_locker(0);
#endif
        }
    }
    //后退方向检测
    
    // sta[0]=xp_agv_io_read(SAFTY_BK_1);
    // sta[1]=xp_agv_io_read(SAFTY_BK_2);
    // if(sta[0]==0||sta[1]==0){
    //     count_bk[1]=aos_now_ms();
    //     if((count_bk[1]-count_bk[0])>SAFTY_touch_time){
    //         /*
    //         if(xp_wheel_get_dir()<0){
    //             dir_bk=xp_wheel_get_dir();
    //             xp_wheel_move(1,0);
    //         }
    //         */
    //         if(safty.bk!=1){
    //             safty.bk=1;
    //             #ifdef BATTERY_VERSION
    //             xp_wheel_set_wheel_speed(0,0);
    //             #endif
    //             println("safty mode bk touched !,sta[0]=%d,sta[1]=%d",sta[0],sta[1]);
    //         }
    //     }
    // }
    // else{
    //     count_bk[0]=aos_now_ms();
    //     if((count_bk[0]-count_bk[1])>SAFTY_relase_touch_time){
    //         /*
    //         if(dir_bk<0){
    //             dir_bk=0;
    //             xp_wheel_move(1,-1);
    //         }*/
    //         if(safty.bk!=0){
    //             safty.bk=0;
    //             println("safty mode bk touch released !");
    //         }
    //     }
    // }

}
#endif

/*
safty module init status check
return:
    1=inited
    0=uninit
*/
static int xp_safty_init_check(void){
    if(safty.init!=1){
        perr("safty module uninit~");
        return 0;
    }
    return 1;
}


/*
check conllision status
*/
void xp_safty_conllision_check(void){
    static u8 flag=0,count1=0;

    if(xp_safty_init_check()==0){
        return;
    }
    if(xp_agv_io_read(SAFTY_collision_io)==SAFTY_io_touch){
        count1=0;
        if(flag==0){
            flag=1;
            safty.conllision=1;
            // xp_wheel_move(0,0);
            #if defined LINE_VERSION
            xp_wheel_set_wheel_locker(1);
            #endif
            xp_agv_io_set(SAFTY_stop_io,0);
            aos_msleep(2000);
            xp_agv_io_set(SAFTY_stop_io,1);
            #if defined LINE_VERSION
            xp_wheel_init(1,115200,10);
            #endif
            println("conllion io touched~,stop agv and lock wheel motor~");
        }
    }
    else{
        if(flag==1){
            count1++;
            if(count1>50){
                flag=0;
                count1=51;
                safty.conllision=0;
                println("conllision io touch status release~");
            }
        }
    }
}



/*
check restore motor locker
*/
void xp_safty_restore_check(void){
    if(xp_safty_init_check()==0){
        return;
    }
    if(safty.fw==0&&safty.bk==0&&safty.conllision==0){
        #if defined LINE_VERSION
        xp_wheel_set_wheel_locker(0);
        #endif
        println("safty restore motor locker~");
    }
}


/*
safty event check thread
*/
void xp_safty_check_thread(void *arg){
    
    aos_msleep(5000);
    if(xp_safty_init_check()==0){
        perr("safty module init fail~ task exit~");
        aos_task_exit(0);
    }
    while(1){
        // xp_safty_check();
        // xp_safty_conllision_check();


        aos_msleep(10);
    }
}

//end of the file

